/**
*  \file can_open.c
*
*  \brief
*
*  \author Dajin Li  <dajin.li@linde-.de>
*
*
*/

/*===============================================[ private includes  ]================================================*/

#include "../var/var_tbl.h"
#include "../xmc4500/can_hw.h"
#include "can_open.h"
#include "../xmc4500/int.h"
#include "../app/task.h"
#include "../pal/types.h"
#include "xmc_flash.h"


/*===============================================[ private defines   ]================================================*/

/*===============================================[ private datatypes ]================================================*/

/*===============================================[ private variables ]================================================*/
static s16 canopen_system_status_flags = 0;
// Working Periodic Service Rates
static s16 kwp_data_index = 0;
static u16 kwp_rx_data_length = 0;
static s16 kwp_data_length = 0;
static s16 kwp_var_index;
static s16 kwp_object_id;
static u8 *msg_response_ptr;
static u32 secacc_code;
static u8 secacc_expected_service;
static enum secacc_state secacc_security_state;
static u16 blkIndex = 0;
static u8 sdo_dlc;
static u16 dataIndex = 0;
static u16 checksum;
static u16 temp_checksum;
static u32 recevie_count;
static u8  *multi_char_ptr = 0;
static u8 multi_period;
static u8 multi_send;
static struct fortune fortune_fate;
/*===============================================[ public variables ]================================================*/
union CAN_SYSTEM_FLAGS CanSystemFlags;
u32 startAddr;
u32 endAddr;
u32 flash_length;
u8 sdo_data[8];
u8 Can_Rx_buf[BUFFER_SIZE];
u8 Mem_Tx_buf[BUFFER_SIZE];
u8 stay_for_ever;



/*===============================================[ private functions ]================================================*/
static s16 setup_kwp(void);
static void process_kwp_mosi(can_mailbox *message);
static u32 secacc_crypto_12( u32 seed );
static u32 secacc_crypto_34( u32 seed);
static u32 secacc_crypto_56( u32 seed);
static enum kwp_response_code secacc_process_request( u8 condition, u8  *dat, u16 *len);
static enum secacc_state secacc_get_state( void );
static u32 secacc_fortuna( void );
static void send_kwp_response(void);
static void send_kwp_init01_response(void);
static s16 lookup_object_id(kwp_data_st *table, s16 length, s32 target);

/*================================================[ inline functions ]================================================*/

/*================================================[ public functions ]================================================*/
/**
* @brief    canopen_init
* @param    none
* @note     none
* @retval   int
*/
s16 canopen_init(void)
{
    u8 result = 1;

    result = setup_kwp();
    if(result == 0) 
    {
        return(0);
    }
    
    canopen_system_status_flags = canopen_system_status_flags | CAN_OPEN_ENABLED;
    
    return(1);
}

/**
* @brief    process CAN message
* @param    CanRxMsg* RxMessage: points to rx message
* @note     none
* @retval   none
*/
void process_msg(can_mailbox *message)
{
    u32 id = message->id.full;
    u16 msgType = id & 0x7FF;

    if(message->extended == XMC_CAN_FRAME_TYPE_EXTENDED_29BITS) // Skip extended message
    {
        return;
    }

    switch(msgType) 
    {
    case NMT:
        break;

    case TRAC_KWP_RX_ID_ALL:
    case TRAC_KWP_RX_ID:
    case TRAC_KWP_INIT01_RX_ID_ALL:
        process_kwp_mosi(message);
        break;

    default:
        break;
    }
    
    return;
}
void Multi_msg_send(void)
{
    u8 i = 0;
    static s8 period = 0;
    static u32 pre_flash_time = 0;

    u32 time = get_sys_tick();

    if(pre_flash_time == time)
    {
        return;
    }
    pre_flash_time = time;
    
    period--;
    if(period > 0)
    {
        return;
    }
    period = multi_period;
    if(multi_send)
    {
        sdo_data[0] = 0x20 | (blkIndex%16);
        if(kwp_data_length >= 7)
        {

            for(i = 1;i < 8;i++)
            {
                sdo_data[i] = multi_char_ptr[kwp_data_index++];
            }
            kwp_data_length -= 7;
            if(kwp_data_length == 0)
            {
                multi_send = 0;
            }
        }
        else
        {
            for(i = 1;i < kwp_data_length+1;i++)
            {
                sdo_data[i] = multi_char_ptr[kwp_data_index++];
            }
            for(i = kwp_data_length+1;i < 8;i++)
            sdo_data[i] = 0;
            multi_send = 0;
            kwp_data_length = 0;
        }
        send_kwp_response();
        blkIndex++;
    }
}


fls_PLACE_IN_RAM void KWP_send_pending_msg(void)
{
    can_mailbox mailbox;


    sdo_data[0] = 3;
    sdo_data[1] = KWP_FAIL;
    sdo_data[2] = 0x31;
    sdo_data[3] = 0x78;
    sdo_data[4] = 0;
    sdo_data[5] = 0;
    sdo_data[6] = 0;
    sdo_data[7] = 0;  
    
    mailbox.store.u8data[0] = sdo_data[0];
    mailbox.store.u8data[1] = sdo_data[1];
    mailbox.store.u8data[2] = sdo_data[2];
    mailbox.store.u8data[3] = sdo_data[3];
    mailbox.store.u8data[4] = sdo_data[4];
    mailbox.store.u8data[5] = sdo_data[5];
    mailbox.store.u8data[6] = sdo_data[6];
    mailbox.store.u8data[7] = sdo_data[7];

    mailbox.extended = XMC_CAN_FRAME_TYPE_STANDARD_11BITS;
    mailbox.id.full = TRAC_KWP_TX_ID; 
    mailbox.dlc = 8;
    
    mailbox.port = CAN_PORT_EXT;
    
    if(send_kwp_msg == 1)
    {
        send_can_message_hw(&mailbox);
    }
}

void KWP_send_boot_msg(void)
{
    can_mailbox mailbox;
    
    mailbox.store.u8data[0] = 0x00;
    mailbox.store.u8data[1] = 0x00;
    mailbox.store.u8data[2] = 0x00;
    mailbox.store.u8data[3] = 0x00;
    mailbox.store.u8data[4] = 0x00;
    mailbox.store.u8data[5] = 0x00;
    mailbox.store.u8data[6] = 0x00;
    mailbox.store.u8data[7] = 0x00;

    mailbox.extended = XMC_CAN_FRAME_TYPE_STANDARD_11BITS;
    mailbox.id.full = 0x4EE; 
    mailbox.dlc = 8;
    
    mailbox.port = CAN_PORT_EXT;

    send_can_message(&mailbox);
}




/*===============================================[ private functions ]================================================*/
/**
* @brief    setup for kwp2000
* @param    none
* @note     none
* @retval   int
*/
static s16 setup_kwp(void)
{
    mailbox_vInit(CAN_PORT_EXT, TRAC_KWP_RX_ID_ALL);
    mailbox_vInit(CAN_PORT_EXT, TRAC_KWP_RX_ID);
    mailbox_vInit(CAN_PORT_EXT, TRAC_KWP_INIT01_RX_ID_ALL);
    return(1);
}


/**
* @brief    send sdo reponse
* @param    s16 type: reponse type
* @param    u32 result: reponse type
* @note     none
* @retval   s16
*/
static void send_kwp_response(void)
{
    can_mailbox mailbox;
    
    mailbox.store.u8data[0] = sdo_data[0];
    mailbox.store.u8data[1] = sdo_data[1];
    mailbox.store.u8data[2] = sdo_data[2];
    mailbox.store.u8data[3] = sdo_data[3];
    mailbox.store.u8data[4] = sdo_data[4];
    mailbox.store.u8data[5] = sdo_data[5];
    mailbox.store.u8data[6] = sdo_data[6];
    mailbox.store.u8data[7] = sdo_data[7];

    mailbox.extended = XMC_CAN_FRAME_TYPE_STANDARD_11BITS;
    mailbox.id.full = TRAC_KWP_TX_ID; 
    mailbox.dlc = 8;
    
    mailbox.port = CAN_PORT_EXT;

    send_can_message(&mailbox);
    return;
}

/**
* @brief    send sdo reponse
* @param    s16 type: reponse type
* @param    u32 result: reponse type
* @note     none
* @retval   s16
*/
static void send_kwp_init01_response(void)
{
    can_mailbox mailbox;
    
    mailbox.store.u8data[0] = sdo_data[0];
    mailbox.store.u8data[1] = sdo_data[1];
    mailbox.store.u8data[2] = sdo_data[2];
    mailbox.store.u8data[3] = sdo_data[3];
    mailbox.store.u8data[4] = sdo_data[4];
    mailbox.store.u8data[5] = sdo_data[5];
    mailbox.store.u8data[6] = sdo_data[6];
    mailbox.store.u8data[7] = sdo_data[7];

    mailbox.extended = XMC_CAN_FRAME_TYPE_STANDARD_11BITS;
    mailbox.id.full = TRAC_KWP_INIT01_TX_ID_ALL; 
    mailbox.dlc = 8;
    mailbox.port = CAN_PORT_EXT; 

    send_can_message(&mailbox);
    return;
}


/**
* @brief    process kwp message
* @param    CanRxMsg* RxMessage: buffer
* @note     none
* @retval   none
*/
static void process_kwp_mosi(can_mailbox *message)
{
    u8 service_id;
    u32 *long_ptr = 0;
    u16 *short_ptr = 0;  
    u8 frame_type = 0;
    u32 i;//, k, m;
    u32 temp_data;
    u8 entry_option;
    u32 min_value;
    u32 max_value;
    u16 attr;
    //HAL_StatusTypeDef status = HAL_OK;
    u16 data_length;
    //u16 word_length;
    //u16 remainder_bytes;

    enum kwp_response_code rv;
//   u16 ee_addr;

    u8 rx_completed = 0;

    if(message == NULL )
    {
        return;
    }


    sdo_data[0] = message->store.u8data[0];
    sdo_data[1] = message->store.u8data[1];
    sdo_data[2] = message->store.u8data[2];
    sdo_data[3] = message->store.u8data[3];
    sdo_data[4] = message->store.u8data[4];
    sdo_data[5] = message->store.u8data[5];
    sdo_data[6] = message->store.u8data[6];
    sdo_data[7] = message->store.u8data[7];

    sdo_dlc = message->dlc;
    

   
    frame_type = (sdo_data[0] & 0xF0) >> 4;


    if(frame_type == KWP_SINGLE_FRM)
    {
        dataIndex = 0;
        for(i = 1; i < sdo_dlc; i++)
        {
            Can_Rx_buf[dataIndex++] = sdo_data[i];
        }
        rx_completed = 1;
        kwp_rx_data_length = sdo_data[0] & 0x0F;
        recevie_count = 0;
        multi_send = 0;
    }
    else if(frame_type == KWP_FIRST_FRM)
    {
        dataIndex = 0;
        for(i = 2; i < sdo_dlc; i++)
        {
            Can_Rx_buf[dataIndex++] = sdo_data[i];
        }
        rx_completed = 0;
        kwp_rx_data_length = ((((s16)sdo_data[0]) & 0x0F) << 8) | sdo_data[1];
        blkIndex = 1;
        sdo_data[0] = 0x30;
        sdo_data[1] = 0;   // default session
        sdo_data[2] = 0x00;   // default session
        sdo_data[3] = 0;
        sdo_data[4] = 0;
        sdo_data[5] = 0;
        sdo_data[6] = 0;
        sdo_data[7] = 0;

        send_kwp_response();
    }
    else if(frame_type == CONSECUTIVE_FRM)
    {
        i = sdo_data[0] & BLK_INDEX_MASK; // Get block index
        if(i == blkIndex)
        {
            for(i = 1; i < sdo_dlc; i++)
            {
                Can_Rx_buf[dataIndex++] = sdo_data[i];
            }
            if(dataIndex >= kwp_rx_data_length)
            {
                rx_completed = 1;
            }
        }
        blkIndex++;
        blkIndex %= 16;
    }
    else if(frame_type == KWP_FLOW_CONTROL_FRM)
    {
        multi_char_ptr = msg_response_ptr;
        multi_send = 1;
        blkIndex = 1;
        multi_period = sdo_data[2];
        if(multi_period == 0)
        {
            multi_period = 2;
        }
    }



    if(rx_completed == 1)
    {
        dataIndex = 0;
        service_id = Can_Rx_buf[dataIndex++];
        switch(service_id)
        {
        case startDiagnosticSession:
            kwp_object_id = Can_Rx_buf[dataIndex++];
            if(kwp_object_id == KWP_DEFAULT_SESSION)
            {
                secacc_security_state = secacc_state_default;                                      // default session
                sdo_data[0] = 2;
                sdo_data[1] = KWP_START_DEFAULT_SESSION_OK;
                sdo_data[2] = KWP_DEFAULT_SESSION;   // default session
                sdo_data[3] = 0;
            }
            else if(kwp_object_id == KWP_START_PROGRAMING_SESSION)
            {
                if (secacc_get_state() != secacc_state_programming)
                {
                    sdo_data[0] = 3;
                    sdo_data[1] = KWP_FAIL;
                    sdo_data[2] = service_id;                       // default session
                    sdo_data[3] = KWP_RESPONSE_CODE_ACCESSDENIED;   // default session
                }
                else
                {
                    sdo_data[0] = 2;
                    sdo_data[1] = KWP_START_DEFAULT_SESSION_OK;
                    sdo_data[2] = kwp_object_id;   // default session
                    sdo_data[3] = 0;
                }
            }
            sdo_data[4] = 0;
            sdo_data[5] = 0;
            sdo_data[6] = 0;
            sdo_data[7] = 0;
            dataIndex = 0;
            send_kwp_response();
            break;
        
        case readECUIdentification:
            kwp_object_id = Can_Rx_buf[dataIndex++];
            kwp_var_index = lookup_object_id(kwp_ecu_params, MAX_KWP_ECU_OBJECT_DEF, kwp_object_id);
            if (kwp_var_index < 0)
            {
                sdo_data[0] = 3;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = service_id;
                sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();
            }
            else
            {
                attr = kwp_ecu_params[kwp_var_index].attr;
                switch(attr & VAR_LENGTH_MASK) 
                {
                case FOUR_BYTES_LEN:
                    long_ptr = (u32 *) kwp_ecu_params[kwp_var_index].var;
                    sdo_data[0] = 5;
                    sdo_data[1] = RD_ECU_OK;
                    sdo_data[2] = (u8)((*long_ptr) & 0x000000FF);
                    sdo_data[3] = (u8)(((*long_ptr) & 0x0000FF00) >> 8);
                    sdo_data[4] = (u8)(((*long_ptr) & 0x00FF0000) >> 16);
                    sdo_data[5] = (u8)(((*long_ptr) & 0xFF000000) >> 24);
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;

                case TWO_BYTES_LEN:                          
                    short_ptr = (u16 *) kwp_ecu_params[kwp_var_index].var;
                    sdo_data[0] = 3;
                    sdo_data[1] = RD_ECU_OK;
                    sdo_data[2] = (u8)((*short_ptr) & 0x000000FF);
                    sdo_data[3] = (u8)(((*short_ptr) & 0x0000FF00) >> 8);
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;

                case ONE_BYTES_LEN:
                    msg_response_ptr = (u8 *) kwp_ecu_params[kwp_var_index].var;
                    sdo_data[0] = 2;
                    sdo_data[1] = RD_ECU_OK;
                    sdo_data[2] = (u8)(*msg_response_ptr);
                    sdo_data[3] = 0;
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;

                default:
                    msg_response_ptr = (u8 *) kwp_ecu_params[kwp_var_index].var;
                    kwp_data_length = strlen((char *)msg_response_ptr);
                    kwp_data_length += 1;
                    kwp_data_index = 0;
                    sdo_data[0] = (1 << 4) | ((kwp_data_length >> 8) & 0x0F);
                    sdo_data[1] = kwp_data_length & 0xFF;
                    sdo_data[2] = RD_ECU_OK;
                    sdo_data[3] = msg_response_ptr[kwp_data_index++];
                    sdo_data[4] = msg_response_ptr[kwp_data_index++];
                    sdo_data[5] = msg_response_ptr[kwp_data_index++];
                    sdo_data[6] = msg_response_ptr[kwp_data_index++];
                    sdo_data[7] = msg_response_ptr[kwp_data_index++];
                    send_kwp_response();
                    kwp_data_length -= 6;

                    break; 
                }
            }
            break;

        case readDataByLocalIdentifier:
            kwp_object_id = Can_Rx_buf[dataIndex++];
            kwp_var_index = lookup_object_id(kwp_data_params, MAX_KWP_DATA_OBJECT_DEF, kwp_object_id);
            if (kwp_var_index < 0)
            {
                sdo_data[0] = 3;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = service_id;
                sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();
            }
            else
            {
                switch(kwp_data_params[kwp_var_index].attr & VAR_LENGTH_MASK) 
                {
                case FOUR_BYTES_LEN:
                    long_ptr = (u32 *) kwp_data_params[kwp_var_index].var;
                    sdo_data[0] = 6;
                    sdo_data[1] = KWP_RD_DATA_OK;
                    sdo_data[2] = kwp_object_id;
                    sdo_data[3] = (u8)((*long_ptr) & 0x000000FF);
                    sdo_data[4] = (u8)(((*long_ptr) & 0x0000FF00) >> 8);
                    sdo_data[5] = (u8)(((*long_ptr) & 0x00FF0000) >> 16);
                    sdo_data[6] = (u8)(((*long_ptr) & 0xFF000000) >> 24);
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;

                case TWO_BYTES_LEN:   
                    short_ptr = (u16 *) kwp_data_params[kwp_var_index].var;
                    sdo_data[0] = 4;
                    sdo_data[1] = KWP_RD_DATA_OK;
                    sdo_data[2] = kwp_object_id;
                    sdo_data[3] = (u8)((*short_ptr) & 0x000000FF);
                    sdo_data[4] = (u8)(((*short_ptr) & 0x0000FF00) >> 8);
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;

                case ONE_BYTES_LEN:
                    msg_response_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                    sdo_data[0] = 3;
                    sdo_data[1] = KWP_RD_DATA_OK;
                    sdo_data[2] = kwp_object_id;
                    sdo_data[3] = (u8)(*msg_response_ptr);
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;

                default:
                    msg_response_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                    kwp_data_length = strlen((char *)msg_response_ptr);
                    kwp_data_length += 2;
                    kwp_data_index = 0;
                    sdo_data[0] = (1 << 4) | ((kwp_data_length >> 8) & 0x0F);
                    sdo_data[1] = kwp_data_length & 0xFF;
                    sdo_data[2] = KWP_RD_DATA_OK;
                    sdo_data[3] = kwp_object_id;
                    sdo_data[4] = msg_response_ptr[kwp_data_index++];
                    sdo_data[5] = msg_response_ptr[kwp_data_index++];
                    sdo_data[6] = msg_response_ptr[kwp_data_index++];
                    sdo_data[7] = msg_response_ptr[kwp_data_index++];
                    send_kwp_response();
                    kwp_data_length -= 6;
                    break; 
                }
            }
            break;
        case writeDataByLocalIdentifier:  // write data
            kwp_object_id = Can_Rx_buf[dataIndex++];
            kwp_var_index = lookup_object_id(kwp_data_params, MAX_KWP_DATA_OBJECT_DEF, kwp_object_id);
            if (kwp_var_index < 0)
            {
                sdo_data[0] = 2;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = service_id;
                sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();
            }
            else
            {
                if((kwp_data_params[kwp_var_index].attr & READ_WRITE) != READ_WRITE)
                {
                    sdo_data[0] = 2;
                    sdo_data[1] = KWP_FAIL;
                    sdo_data[2] = service_id;
                    sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    break;
                }

                attr = kwp_data_params[kwp_var_index].attr;
                min_value = kwp_data_params[kwp_var_index].min;
                max_value = kwp_data_params[kwp_var_index].max;

                switch(attr & VAR_LENGTH_MASK) 
                {
                case FOUR_BYTES_LEN:
                    temp_data = sdo_data[3];
                    temp_data |= (((u32)sdo_data[4]) << 8);
                    temp_data |= (((u32)sdo_data[5]) << 16);
                    temp_data |= (((u32)sdo_data[6]) << 24);

                    if((((s32)temp_data < (s32)min_value || (s32)temp_data > (s32)max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == SIGNED_VAR)) || 
                        ((temp_data < min_value || temp_data > max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == 0)))
                    {
                        sdo_data[0] = 2;
                        sdo_data[1] = KWP_FAIL;
                        sdo_data[2] = service_id;
                        sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                        sdo_data[4] = 0;
                        sdo_data[5] = 0;
                        sdo_data[6] = 0;
                        sdo_data[7] = 0;
                    }
                    else
                    {
                        long_ptr = (u32 *) kwp_data_params[kwp_var_index].var;
                        *long_ptr = temp_data;
                        sdo_data[0] = 3;
                        sdo_data[1] = KWP_WR_DATA_OK;
                        sdo_data[2] = kwp_object_id;
                        sdo_data[3] = 0;
                        sdo_data[4] = 0;
                        sdo_data[5] = 0;
                        sdo_data[6] = 0;
                        sdo_data[7] = 0;
                        write_parameter_to_fram(kwp_var_index);
                    }
                    send_kwp_response();
                    break;

                case TWO_BYTES_LEN:                          
                    temp_data = sdo_data[3];
                    temp_data |= (((u16)sdo_data[4]) << 8);
                    
                    if((((s16)temp_data < (s16)min_value || (s16)temp_data > (s16)max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == SIGNED_VAR)) || 
                        ((temp_data < min_value || temp_data > max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == 0)))    
                    {
                        sdo_data[0] = 2;
                        sdo_data[1] = KWP_FAIL;
                        sdo_data[2] = service_id;
                        sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                        sdo_data[4] = 0;
                        sdo_data[5] = 0;
                        sdo_data[6] = 0;
                        sdo_data[7] = 0;
                    }
                    else
                    {
                        short_ptr = (u16 *) kwp_data_params[kwp_var_index].var;
                        *short_ptr = temp_data;
                        sdo_data[0] = 3;
                        sdo_data[1] = KWP_WR_DATA_OK;
                        sdo_data[2] = kwp_object_id;
                        sdo_data[3] = 0;
                        sdo_data[4] = 0;
                        sdo_data[5] = 0;
                        sdo_data[6] = 0;
                        sdo_data[7] = 0;
                        write_parameter_to_fram(kwp_var_index);
                    }
                    send_kwp_response();
                    break;
                case ONE_BYTES_LEN:
                    temp_data = sdo_data[3];
                    
                    if((((s8)temp_data < (s8)min_value || (s8)temp_data > (s8)max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == SIGNED_VAR)) || 
                        ((temp_data < min_value || temp_data > max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == 0)))         
                    {
                        sdo_data[0] = 2;
                        sdo_data[1] = KWP_FAIL;
                        sdo_data[2] = service_id;
                        sdo_data[3] = KWP_FUN_NOT_SUPPORT;
                        sdo_data[4] = 0;
                        sdo_data[5] = 0;
                        sdo_data[6] = 0;
                        sdo_data[7] = 0;
                    }
                    else
                    {
                        msg_response_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                        *msg_response_ptr = temp_data;
                        sdo_data[0] = 3;
                        sdo_data[1] = KWP_WR_DATA_OK;
                        sdo_data[2] = kwp_object_id;
                        sdo_data[3] = 0;
                        sdo_data[4] = 0;
                        sdo_data[5] = 0;
                        sdo_data[6] = 0;
                        sdo_data[7] = 0;
                        write_parameter_to_fram(kwp_var_index);
                    }
                    send_kwp_response();
                    break;

                default:
                    data_length = kwp_rx_data_length - 2;
                    msg_response_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                    for(i = 0; i < data_length; i++)
                    {
                        msg_response_ptr[i] = Can_Rx_buf[dataIndex++];
                    }

                    if(data_length < (attr & VAR_LENGTH_MASK))
                    {
                        msg_response_ptr[data_length] = 0x00; // Add end character
                    }
                    write_parameter_to_fram(kwp_var_index);
                    sdo_data[0] = 3;
                    sdo_data[1] = KWP_WR_DATA_OK;
                    sdo_data[2] = kwp_object_id;
                    sdo_data[3] = 0;
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                    
                    break; 
                }
            }
            break;
        case securityAccess:
            data_length = kwp_rx_data_length - 1;
            rv = secacc_process_request(TRUE, &Can_Rx_buf[dataIndex], &data_length);

            if(rv == 0)
            {
                sdo_data[0] = data_length + 1;
                sdo_data[1] = KWP_SECURITY_OK;
                
                for( i = 2; i < data_length + 2; i++)
                {
                    sdo_data[i] = Can_Rx_buf[i - 1];
                }
                
            }
            else
            {
                sdo_data[0] = 3;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = securityAccess;
                sdo_data[3] = rv;
                data_length = 2;
            }
            for(i = data_length + 2; i < 8; i++)
            {
                sdo_data[i] = 0;
            }
            send_kwp_response();
            break;

        case startRoutineByLocalIdentifier:
            kwp_object_id = Can_Rx_buf[dataIndex++];
            entry_option = Can_Rx_buf[dataIndex++];
            if((kwp_object_id == KWP_START_FLASH) && (entry_option == 0)) // start flash
            {
                sdo_data[0] = 2;
                sdo_data[1] = startRoutineByLocalIdentifier + 0x40;
                sdo_data[2] = kwp_object_id;
                sdo_data[3] = 0;
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();
            }
            else if(kwp_object_id == KWP_DEL_FLASH) // delete flash
            {
                startAddr = entry_option;
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 24);
                flash_length = (((u32)Can_Rx_buf[dataIndex++]));
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 24);

                if((startAddr < ApplicationSWAddress) || (flash_length > AppLength) || (startAddr %4 != 0))
                {
                    sdo_data[0] = 3;
                    sdo_data[1] = 0x7F;
                    sdo_data[2] = startRoutineByLocalIdentifier;
                    sdo_data[3] = KWP_RESPONSE_CODE_SUBFUNCTIONNOTSUPPORTED;
                }
                else
                {
                    for(i = 0; i < 11; i++)
                    {
                        part_number[i] = 0x00;
                    }
                    part_number[11] = 0;
                    write_parameter_to_fram(0);
                    

                    EraseFlash((startAddr & 0xffffff) | XMC_FLASH_UNCACHED_BASE, flash_length);

                    sdo_data[0] = 3;
                    sdo_data[1] = startRoutineByLocalIdentifier + 0x40;
                    sdo_data[2] = kwp_object_id;
                    sdo_data[3] = 0x01;
                }
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();
            } 
            else if(kwp_object_id == KWP_CAL_CHECKSUM) // flash checksum
            {
                startAddr = entry_option;
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 24);
                
                endAddr = (((u32)Can_Rx_buf[dataIndex++]));
                endAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                endAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                endAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 24);
                
                checksum = (((u32)Can_Rx_buf[dataIndex++]));
                checksum |= (((u32)Can_Rx_buf[dataIndex++]) << 8);


                if((startAddr < ApplicationSWAddress) || (startAddr > AppEndAddress))
                {
                    sdo_data[0] = 3;
                    sdo_data[1] = KWP_FAIL;
                    sdo_data[2] = kwp_object_id;
                    sdo_data[3] = KWP_RESPONSE_CODE_INVALIDFORMAT;
                }
                else if((endAddr < ApplicationSWAddress) || (endAddr > AppEndAddress) || (startAddr > endAddr))
                {
                    sdo_data[0] = 3;
                    sdo_data[1] = KWP_FAIL;
                    sdo_data[2] = kwp_object_id;
                    sdo_data[3] = KWP_RESPONSE_CODE_INVALIDFORMAT;
                }
                else
                {
                    temp_checksum = 0;
                    while(startAddr <= endAddr)
                    {
                        temp_checksum += *(u8 *)startAddr;
                        startAddr++;
                    }

                    if(temp_checksum != checksum)
                    {
                        //EraseFlash(startAddr, endAddr - startAddr);
                        sdo_data[0] = 3;
                        sdo_data[1] = KWP_FAIL;
                        sdo_data[2] = kwp_object_id;
                        sdo_data[3] = KWP_RESPONSE_CODE_INVALIDCHECKSUM;
                    }
                    else
                    {
                        sdo_data[0] = 3;
                        sdo_data[1] = startRoutineByLocalIdentifier + 0x40;
                        sdo_data[2] = kwp_object_id;
                        sdo_data[3] = 0x00;
                    }
                }
    
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();
            } 
            else if(kwp_object_id == KWP_PASSIVE_MODE) // passive mod
            {
                if(entry_option == 0x00)
                {
                    sdo_data[0] = 2;
                    sdo_data[1] = startRoutineByLocalIdentifier + 0x40;
                    sdo_data[2] = 0x04;
                    sdo_data[3] = 0;
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                }
            }
            else if((kwp_object_id == KWP_INIT01)) // init01
            {
                if(entry_option == KWP_EMERGENCY_FLASH)
                {
                    sdo_data[0] = 2;
                    sdo_data[1] = startRoutineByLocalIdentifier + 0x40;
                    sdo_data[2] = 0x80;
                    sdo_data[3] = 0;
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_init01_response();

                    if(stay_for_ever == 0)
                    {
                        stay_for_ever = 1;
                    }
                }
            }
            
            break;
        case requestDownload:                                           // request download
            if ( secacc_get_state() != secacc_state_programming )
            {
                sdo_data[0] = 3;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = requestDownload;
                sdo_data[3] = KWP_RESPONSE_CODE_ACCESSDENIED;   // can transfer 256 bytes
            }
            else
            {
                
                startAddr = (u32)Can_Rx_buf[dataIndex++];
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 24);

                dataIndex++; // skip invalid data;

                flash_length = (((u32)Can_Rx_buf[dataIndex++]));
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 24);

                startAddr = (startAddr & 0xffffff)  | XMC_FLASH_UNCACHED_BASE;

                sdo_data[0] = 3;
                sdo_data[1] = requestDownload + 0x40;
                sdo_data[2] = 0;
                sdo_data[3] = 1;   // can transfer 256 bytes
            }
            sdo_data[4] = 0;
            sdo_data[5] = 0;
            sdo_data[6] = 0;
            sdo_data[7] = 0;
            send_kwp_response();
            
            break;
        case transferData:                                             // transfer data
            if ( secacc_get_state() != secacc_state_programming )
            {
                sdo_data[0] = 3;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = requestDownload;
                sdo_data[3] = KWP_RESPONSE_CODE_ACCESSDENIED;   // can transfer 256 bytes
                sdo_data[4] = 0;
                sdo_data[5] = 0;
            }
            else
            {
                data_length = kwp_rx_data_length - 1;
                recevie_count += data_length;

                if(data_length == 0x100)  // one page
                {
                    XMC_FLASH_ProgramPage((u32 *)startAddr , (u32 *)(&Can_Rx_buf[1]));
                    startAddr += 0x0100;
                }
 
                sdo_data[0] = 5;
                sdo_data[1] = transferData + 0x40;
                sdo_data[2] = recevie_count & 0xFF;
                sdo_data[3] = (recevie_count >> 8) & 0xFF;   // can transfer 256 bytes
                sdo_data[4] = (recevie_count >> 16) & 0xFF;   // can transfer 256 bytes
                sdo_data[5] = (recevie_count >> 24) & 0xFF;   // can transfer 256 bytes
            }    
            sdo_data[6] = 0;
            sdo_data[7] = 0;
            send_kwp_response();
            break;
        case requestTransferExit:                                       // exit transfer
            if ( secacc_get_state() != secacc_state_programming )
            {
                sdo_data[0] = 3;
                sdo_data[1] = KWP_FAIL;
                sdo_data[2] = requestTransferExit;
                sdo_data[3] = KWP_RESPONSE_CODE_ACCESSDENIED;   // can transfer 256 bytes
            }
            else
            {
                startAddr = (u32)Can_Rx_buf[dataIndex++];
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                startAddr |= (((u32)Can_Rx_buf[dataIndex++]) << 24);

                dataIndex++; // skip invalid data;

                flash_length = (((u32)Can_Rx_buf[dataIndex++]));
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 8);
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 16);
                flash_length |= (((u32)Can_Rx_buf[dataIndex++]) << 24);

                sdo_data[0] = 1;
                sdo_data[1] = requestTransferExit + 0x40;
                sdo_data[2] = 0;
                sdo_data[3] = 0;   // can transfer 256 bytes
            }
            sdo_data[4] = 0;
            sdo_data[5] = 0;
            sdo_data[6] = 0;
            sdo_data[7] = 0;
            send_kwp_response();
            break;

        case ecuReset:                                       // exit transfer
            kwp_object_id = Can_Rx_buf[dataIndex++];

            if(kwp_object_id == 0x01)
            {
                sdo_data[0] = 1;
                sdo_data[1] = ecuReset + 0x40;
                sdo_data[2] = 0;
                sdo_data[3] = 0;
                sdo_data[4] = 0;
                sdo_data[5] = 0;
                sdo_data[6] = 0;
                sdo_data[7] = 0;
                send_kwp_response();


                for( i = 0; i < 1000000; i++)
                {
                    __NOP();
                }
                NVIC_SystemReset();
            }
            break;
        case stopDiagnosticSession:                                       // exit transfer
            sdo_data[0] = 1;
            sdo_data[1] = stopDiagnosticSession + 0x40;
            sdo_data[2] = 0;
            sdo_data[3] = 0;
            sdo_data[4] = 0;
            sdo_data[5] = 0;
            sdo_data[6] = 0;
            sdo_data[7] = 0;
            send_kwp_response();
            break;
        case testerPresent:                                       // tester present
            {
                kwp_object_id = Can_Rx_buf[dataIndex++];
                if(kwp_object_id == 1)
                {
                    sdo_data[0] = 1;
                    sdo_data[1] = testerPresent + 0x40;
                    sdo_data[2] = 0;
                    sdo_data[3] = 0;
                    sdo_data[4] = 0;
                    sdo_data[5] = 0;
                    sdo_data[6] = 0;
                    sdo_data[7] = 0;
                    send_kwp_response();
                }
                break;
            }
        default:
            break;
        }
    }
}
/** Key Calculation for Security Access 1/2.
*
*  \param seed Input value.
*
*  \return Calculated Key.
*/
static u32 secacc_crypto_12( u32 seed )
{
   /* calculate key */
   const u32 a = 0x974c58ab;
   const u32 b = 0xfedcba98;
   const u32 c = 0x98765432;
   u32 x = seed + a;

   if( (0xffffffff - a) < seed)
   {
      x ^= b;
   }
   else
   {
      x ^= c;
   }

   x = (x >> 11) | ((x & 0x7ff) << 21);

   return x;
}

/** Key Calculation for Security Access 3/4.
*
*  \param seed Input value.
*
*  \return Calculated Key.
*/
static u32 secacc_crypto_34( u32 seed )
{
   return seed + 0x1AF7;
}

/** Key Calculation for Security Access 5/6.
*
*  \param seed Input value.
*
*  \return Calculated Key.
*/
static u32 secacc_crypto_56( u32 seed )
{
   u32 rv;
   u8 data[8];

   data[3] = (u8)seed;
   seed >>= 8;
   data[2] = (u8)seed;
   seed >>= 8;
   data[1] = (u8)seed;
   seed >>= 8;
   data[0] = (u8)seed;

   data[0] += 0x97;
   data[1] += 0x28;
   data[2] += 0xAA;
   data[3] += 0x55;

   rv = data[3];
   rv <<= 8;
   rv |= data[2];
   rv <<= 8;
   rv |= data[1];
   rv <<= 8;
   rv |= data[0];

   return rv;
}
/** Calculate Key
*
*  \param seed Input value.
*  \param state Kind of algorithm.
*
*  \return Calculated Key
*/
static u32 secacc_calculate_key( u32 seed, enum secacc_state state )
{
   switch(state)
   {
      case secacc_state_programming: return secacc_crypto_12( seed );
      case secacc_state_service    : return secacc_crypto_34( seed );
      case secacc_state_eopl       : return secacc_crypto_56( seed );
      default                      : return 0; /* will never happen */
   }
}
/** Copy 32Bit word into 4 Byte array.
*
*  \param dat Pointer to the destination.
*  \param value To by copied.
*/
static void secacc_u32_copy( u8 *dat, u32 value )
{
   dat[3] = (u8)(value >> 24);
   dat[2] = (u8)(value >> 16);
   dat[1] = (u8)(value >> 8);
   dat[0] = (u8)value;
}
static enum kwp_response_code secacc_seed( u8 condition, u8  *dat, u16 *len, enum secacc_state state)
{
   enum kwp_response_code rv;
   u32 seed;
   if (*len != 1) /* Wrong Length */
   {
      rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
   }
   else
   {
      if( FALSE == condition )
      {
         rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
      }
      else
      {
         if( state == secacc_security_state )
         {
            /* ECU already unlocked */
            dat[1] = 0;
            dat[2] = 0;
            *len   = 3;
            rv = KWP_RESPONSE_CODE_NONE;
         }
         else
         {
          seed = secacc_fortuna();
          secacc_code = secacc_calculate_key( seed, state );
          secacc_expected_service = dat[0]+1;
          *len = 5;
          secacc_u32_copy( &dat[1], seed );
          rv = KWP_RESPONSE_CODE_NONE;
         }
      }
   }
   return rv;
   
}


/**  Read 32 Bit word from given address.
*
* \param dat Source Pointer
*
* \return 32Bit value
*/
static u32 secacc_u32(u8 *dat )
{
   return ((u32)dat[3]<<24)+((u32)dat[2]<<16)+((u32)dat[1]<<8)+dat[0];
}



/** Process Key Request */
static enum kwp_response_code secacc_key( u8 *dat, u16 *len, enum secacc_state state)
{
   enum kwp_response_code rv;

   if (*len != 5)
   {  /* Invalid length */
      rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
   }
   else
   {
      if ( secacc_expected_service == dat[0])
      {
         /* Securitysecacc_state_access in progress */
         if( secacc_u32(&dat[1]) == secacc_code  )
         {  /* valid key */
            secacc_security_state = state;
            *len = 1;
            rv = KWP_RESPONSE_CODE_NONE;
         }
         else
         {  /* invalid key */
            rv = KWP_RESPONSE_CODE_INVALIDKEY;
         }
      }
      else
      {  /* request sequence error */
         rv = KWP_RESPONSE_CODE_REQUESTSEQUENCEERROR;
      }
   }
   return rv;
}
/**  Read 32 Bit word from given address.
*
* \param dat Source Pointer
*
* \return 32Bit value
*/

static enum kwp_response_code secacc_process_request( u8 condition, u8  *dat, u16 *len)
{
   switch( dat[0] )
   {
#if SECACC_12
      case 0x01:  return secacc_seed( condition, dat, len, secacc_state_programming );
      case 0x02:  return secacc_key ( dat, len, secacc_state_programming );
#endif

#if SECACC_34
      case 0x03:  return secacc_seed( TRUE,  dat, len, secacc_state_service);
      case 0x04:  return secacc_key ( dat, len, secacc_state_service );
#endif

#if SECACC_56
      case 0x05:  return secacc_seed( TRUE, dat, len, secacc_state_eopl);
      case 0x06:  return secacc_key ( dat, len, secacc_state_eopl );
#endif
      default:    return KWP_RESPONSE_CODE_SUBFUNCTIONNOTSUPPORTED;
   }
}

static enum secacc_state secacc_get_state( void )
{
   return secacc_security_state;
}

static u16 fortune_u16( struct fortune *fate )
{
   if( NULL == fate)
   {
      fate = &fortune_fate;
   }
   fate->random = fate->random * 1103515245 + 12345;
   return (u16)(fate->random/65536);
}


static void fortune_set(struct fortune *fate, void *dest, u32 size )
{
   u8 *d=dest;
   while(size--)
   {
      *d++=(u8) fortune_u16(fate);
   }
}

static u32 fortune_u32( struct fortune *fate )
{
    u32 rv;
    fortune_set(fate, &rv, sizeof(rv));
    return rv;
}

static void fortune_initialize(struct fortune *fate, u32 value)
{
   if( NULL == fate)
   {
      fate = &fortune_fate;
   }
   fate->random=value;
}


static u32 secacc_fortuna( void )
{
   struct fortune fortune_obj;
   fortune_initialize( &fortune_obj, get_sys_tick());
   return fortune_u32( &fortune_obj );
}

/**
* @brief    lookup KWP2000 obejct id 
* @param    const s32 *table
* @param    s16 length
* @param    s32 target
* @note     none
* @retval   none
*/
static s16 lookup_object_id(kwp_data_st *table, s16 length, s32 target)
{
    int high = length;
    int low = -1;
    int probe = 0;

    while (high - low > 1) 
    {                                  
        probe = (high + low) / 2;       
        if (table[probe].kwp_id < target)
        {
            low = probe;
        }
        else
        {
            high = probe;
        }
    }

    if (high == length || table[high].kwp_id != target) 
    {
        return -1;                     
    }
    else
    {
        return high;                    
    }
}



/***********************************************************************************************************************
*                             Copyright 2016 Linde Material Handling. All rights reserved.                             *
***********************************************************************************************************************/

